#include "mex.h"
#include<stdio.h>
int e,s,t,st,N,S,T,Snew,Tnew,scalex,scalez;
double *idx,*idz,*dx,*dz,*x,*z,*xnew,*znew;
double *mscalar,*mx,*mz;
double *grid,*weight;

int IN0(int s,int t) {
    return t*Snew+s;
}

void markertorhogrid(int e,double value,double *grid,double *weight) {
    //gives a weighted contribution from a value on a marker to the four sorrounding rho grid points. Division with weights must be performed later
    int s,t;
    int INNV,INNE,INSV,INSE;
    double dlx,dlz,dw;
    s=get_s(mx[e]); t=get_t(mz[e]);
    INNV=IN0(s,t); INNE=IN0(s+1,t); INSV=IN0(s,t+1); INSE=IN0(s+1,t+1);
    dlx=(mx[e]-x[s])/dx[s];
    dlz=(mz[e]-z[t])/dz[t];
    // NV
    dw=(1-dlx)*(1-dlx)+(1-dlz)*(1-dlz); if (dw<weight[INNV]) { grid[INNV]=value; weight[INNV]=dw; }
    // NE
    dw=dlx*dlx+(1-dlz)*(1-dlz); if (dw<weight[INNE]) { grid[INNE]=value; weight[INNE]=dw; }
    // SV
    dw=(1-dlx)*(1-dlx)+dlz*dlz; if (dw<weight[INSV]) { grid[INSV]=value; weight[INSV]=dw; }
    // SE
    dw=dlx*dlx+dlz*dlz; if (dw<weight[INSE]) { grid[INSE]=value; weight[INSE]=dw; }
}

int getnearestmarker(double xpos,double zpos) {
    int nearestmarker;
    double dist2;
    double mindist2=DBL_MAX;
    for (e=0;e<N;e++) {
        dist2=(xpos-mx[e])*(xpos-mx[e])+(zpos-mz[e])*(zpos-mz[e]);
        if (dist2<mindist2) {mindist2=dist2; nearestmarker=e; }
    }
    return nearestmarker;
}

void interpolate() {
    for (e=0;e<N;e++)  markertorhogrid(e,mscalar[e],grid,weight);
    for (s=0;s<Snew;s++) {
        for (t=0;t<Tnew;t++) {
            st=IN0(s,t);
            if (weight[st]==DBL_MAX) grid[st]=mscalar[getnearestmarker(x[s],z[t])];
        }
    }
}


int get_s(double xpos) {
    int low,high,mid;
    low=0; high=Snew-1;
    while (high-low>1) { mid=(high+low)/2; if (x[mid]<=xpos) low=mid; else high=mid; }
    return low;
}
int get_t(double zpos) {
    int low,high,mid;
    low=0; high=Tnew-1;
    while (high-low>1) { mid=(high+low)/2; if (z[mid]<=zpos) low=mid; else high=mid; }
    return low;
}

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
    mscalar=mxGetPr(prhs[0]);
    mx=mxGetPr(prhs[1]);
    mz=mxGetPr(prhs[2]);
    idx=mxGetPr(prhs[3]);
    idz=mxGetPr(prhs[4]);
    scalex=(int)*mxGetPr(prhs[5]);
    scalez=(int)*mxGetPr(prhs[6]);
    
    N=mxGetNumberOfElements(prhs[0]);
    S=mxGetNumberOfElements(prhs[3])+1; Snew=scalex*(S-1)+1;
    T=mxGetNumberOfElements(prhs[4])+1; Tnew=scalez*(T-1)+1;
    
    plhs[0]=mxCreateDoubleMatrix(Snew,Tnew, mxREAL);
    plhs[1]=mxCreateDoubleMatrix(Snew,1, mxREAL);
    plhs[2]=mxCreateDoubleMatrix(Tnew,1, mxREAL);
    grid=mxGetPr(plhs[0]); x=mxGetPr(plhs[1]); z=mxGetPr(plhs[2]);
    
    dx=mxCalloc(Snew-1,sizeof(double)); dz=mxCalloc(Tnew-1,sizeof(double));
    weight=mxCalloc(Snew*Tnew,sizeof(double));
    
    for (s=0;s<S-1;s++) {
        for (e=0;e<scalex;e++) dx[scalex*s+e]=idx[s]/scalex; }
    for (t=0;t<T-1;t++) {
        for (e=0;e<scalez;e++) dz[scalez*t+e]=idz[t]/scalez; }

    x[0]=0; for (s=0;s<Snew-1;s++) x[s+1]=x[s]+dx[s];
    z[0]=0; for (t=0;t<Tnew-1;t++) z[t+1]=z[t]+dz[t];
    
    for (s=0;s<Snew;s++) {  for (t=0;t<Tnew;t++) { st=IN0(s,t); grid[st]=0; weight[st]=DBL_MAX; } }
    interpolate();
    mxFree(weight); mxFree(dx); mxFree(dz);
}
